﻿# include "utils.h"
using namespace std;

double norm(Point a) {    //向量的模
	return sqrt(a.x * a.x + a.y * a.y);
}

double norm(const Line& a) {
	Point Sp = a.Pt1;
	Point Ep = a.Pt2;
	return norm(Ep - Sp);
}

double cross(Point a, Point b) {    //叉乘
	return a.x * b.y - a.y * b.x;
}


//计算任意两个线段的最近距离
double getTwoLinesDist(Line line1, Line line2) {
	if (crossSegs(line1, line2)) {
		return 0;
	}

	double DistLeft_1, DistLeft_2, DistRight_1, DistRight_2;
	double linesDistance;
	vector<double> distNums;

	DistLeft_1 = pointToSegmentDistance(line1.Pt1, line2);
	DistLeft_2 = pointToSegmentDistance(line1.Pt2, line2);
	DistRight_1 = pointToSegmentDistance(line2.Pt1, line1);
	DistRight_2 = pointToSegmentDistance(line2.Pt2, line1);

	distNums.emplace_back(DistLeft_1);
	distNums.emplace_back(DistLeft_2);
	distNums.emplace_back(DistRight_1);
	distNums.emplace_back(DistRight_2);

	linesDistance = backMinDistNum(distNums);

	return linesDistance;
}

//从一堆距离集合里面返回最小的一个值(一堆数中选个最小的)
double backMinDistNum(vector<double> DistNums) {
	double minDistance;
	double tempDistance;
	//Init
	minDistance = DistNums[0];
	for (int i = 1; i < DistNums.size(); ++i) {
		tempDistance = DistNums[i];
		if (tempDistance < minDistance) {
			minDistance = tempDistance;
		}
	}

	return minDistance;
}

//计算一个点到一条线段的最短距离
double pointToSegmentDistance(Point basePoint, Line baseLine) {
	bool checkBaseProjection;
	double lessDistance;
	Point baseProjectionPoint, nearestPoint;

	baseProjectionPoint = getProjectionPoint(basePoint, baseLine);
	checkBaseProjection = checkPointInSegment(baseProjectionPoint, baseLine);

	if (checkBaseProjection) {
		lessDistance = linearDist(baseProjectionPoint, basePoint);
	}
	else {
		nearestPoint = getLineNearestPoint(baseProjectionPoint, baseLine);
		lessDistance = linearDist(nearestPoint, basePoint);
	}

	return lessDistance;
}
//求出投影点的位置
Point getProjectionPoint(Point basePoint, Line baseLine) {
	Point baseLineVec, baseLineVecticalVec;

	baseLineVec = baseLine.Pt2 - baseLine.Pt1;
	baseLineVecticalVec = rotate(baseLineVec, 90);

	return getInsection(baseLineVecticalVec, basePoint, baseLineVec, baseLine.Pt1);

}

Point RotateVec(Point vec, double angle) {
	double rad = angle * PI / 180.0;
	Point rotate_vec = { vec.x * cos(rad) - vec.y * sin(rad), vec.y * cos(rad) + vec.x * sin(rad) };
	return rotate_vec;
}

Point getInsection(Point Vec1, Point p1, Point Vec2, Point p2) {
	Point P1P2 = p2 - p1;
	double t = cross(P1P2, Vec2) / cross(Vec1, Vec2);
	Point outPoint = p1 + Vec1 * t; 

	return outPoint;
}
///向量旋转,[1] > 0:逆时针
Point rotate(Point vec, double angle) {
	double rad = angle * PI / 180.0;
	Point rotate_vec = { vec.x * cos(rad) - vec.y * sin(rad), vec.y * cos(rad) + vec.x * sin(rad) };
	return rotate_vec;
}


// 优化判断：两条线段是否有交点  
bool crossSegs(Line line1, Line line2) {
	bool insectPoint;
	bool judge1, judge2;
	Point crossPoint;
	Point vecLine1, vecLine2;
	vecLine1 = line1.Pt2 - line1.Pt1;
	vecLine2 = line2.Pt2 - line2.Pt1;
	NormalizeVec(vecLine1);
	NormalizeVec(vecLine2);
	double t = cross(vecLine1, vecLine2);
	if (abs(t) < CountMinValue) {  // 平行线
		return false;  //无交点
	}

	// if EndingPoint On OtherSegment == Cross  !!! Adding it will result in many points that cannot be calculated and found
	//if (judgeCrossPoint(line2.Pt1, line1) || judgeCrossPoint(line2.Pt2, line1)
	//	|| judgeCrossPoint(line1.Pt1, line2) || judgeCrossPoint(line1.Pt2, line2)) {
	//	return true;
	//}

	crossPoint = getInsection(vecLine1, line1.Pt1, vecLine2, line2.Pt2);  // 交点
	judge1 = judgeCrossPoint(crossPoint, line1);
	judge2 = judgeCrossPoint(crossPoint, line2);
	if (judge1 && judge2) {
		insectPoint = true;
	}
	else {
		insectPoint = false;
	}

	return insectPoint;
}

// 判断点是否在线段中间
bool judgeCrossPoint(Point point, Line line) {

	if (point == line.Pt1 || point == line.Pt2) {
		return true;
	}

	Point sP, eP;
	bool judgeResult = false;
	sP = line.Pt1;
	eP = line.Pt2;

	Point vec1_ = sP - point;  // 2023/11/7 更新新的判断方法
	Point vec2_ = eP - point;
	NormalizeVec(vec1_);
	NormalizeVec(vec2_);
	double t = cross(vec1_, vec2_);
	if (abs(t) > CountMinValue) {  // 不平行
		return false;
	}

	if (pointToLineDist(point, line) > MapMinValue) {
		return false;
	}

	if ((vec1_ * vec2_) < 0) {
		return true;
	}
	else {
		return false;
	}

	//double distL, distR;
	//double segmentLength;
	//distL = linearDist(point, sP);
	//distR = linearDist(point, eP);
	//segmentLength = linearDist(sP, eP);
	//if (abs(segmentLength - (distL + distR)) < MapMinValue) {    // 距离很接近也直接按相等处理
	//	return true;
	//}
	//else {
	//	return false;
	//}

}


double CrossVecs(Point a, Point b) {    //叉乘
	return a.x * b.y - a.y * b.x;
}

void NormalizeVec(Point& NormalVec) {
	double VecLength = sqrt(NormalVec.x * NormalVec.x + NormalVec.y * NormalVec.y);
	if (VecLength != 0 && VecLength != 1) {
		NormalVec = NormalVec / VecLength;
	}
}

//点到直线的距离
double pointToLineDist(Point point, Line line1) {
	Point vecLine;
	Point vecNormalLine;
	Point crossingPoint;
	vecLine = line1.Pt2 - line1.Pt1;
	vecNormalLine = rotate(vecLine, 90);
	crossingPoint = getInsection(vecLine, line1.Pt1, vecNormalLine, point);
	double dist = linearDist(point, crossingPoint);

	return dist;
}


//得到两点之间的直线距离
double linearDist(Point point1, Point point2) {
	double distance;
	distance = sqrt((point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y));
	return distance;
}

//在baseL中找到和baseP最近的端点
Point getLineNearestPoint(Point baseP, Line baseL) {
	double leftDistance, rightDistance;

	leftDistance = rightDistance = DoubleMaxNumValue;
	leftDistance = linearDist(baseP, baseL.Pt1);
	rightDistance = linearDist(baseP, baseL.Pt2);

	if (leftDistance < rightDistance) {
		return baseL.Pt1;
	}
	else {
		return baseL.Pt2;
	}
}



//检查一个点是否在一条线段上面
bool checkPointInSegment(Point tP, Line segmentL) {
	double distL, distR;
	double segmentLength;
	distL = linearDist(tP, segmentL.Pt1);
	distR = linearDist(tP, segmentL.Pt2);
	segmentLength = linearDist(segmentL.Pt1, segmentL.Pt2);
	if (abs(segmentLength - (distL + distR)) < MapMinValue) {  //距离很接近也直接按相等处理
		return true;
	}
	else {
		return false;
	}
}
void exchangePoints(Point& point1, Point& point2) {
	Point tempP;
	tempP = point1;
	point1 = point2;
	point2 = tempP;
}
/*
* @brief 用来判断推挤线是否超过相邻线段的水平位置
* @line 存放推挤线
* @point 相邻线段的固定点
* @return 1： 表明固定点在推挤线的逆时针方向  -1：表明固定点在推挤线的顺时针方向  0：同向
*/
int direction(Line& line, Point& point) {
	double c = cross(line.Pt2 - line.Pt1, point - line.Pt1);
	if (c > 0)
		return 1;        //两向量夹角为正
	else if (c < 0)
		return -1;  //两向量夹角为负
	else
		return 0;
}
// 两条线是否是平行线
bool isParallel_lines(Line line1, Line line2) {
	bool resultJudge;//output
	double t;
	Point vec1, vec2;
	vec1 = line1.Pt2 - line1.Pt1;
	vec2 = line2.Pt2 - line2.Pt1;
	NormalizeVec(vec1);
	NormalizeVec(vec2);
	t = cross(vec1, vec2);
	if (abs(t) < CountMinValue) {
		resultJudge = true;
	}
	else {
		resultJudge = false;
	}
	return resultJudge;
}
bool CheckLinesPerpendicular(Line firstSeg, Line SecondSeg) {
	Point vec1 = firstSeg.Pt2 - firstSeg.Pt1;
	Point vec2 = SecondSeg.Pt2 - SecondSeg.Pt1;
	vec1 = vec1 / norm(vec1);
	vec2 = vec2 / norm(vec2);

	double dotRes = vec1 * vec2;

	if (abs(dotRes) < CountMinValue) {
		return true;
	}
	else {
		return false;
	}
}
bool inspectVectorsDirection(Point vec1, Point vec2) {
	double dotResult;
	NormalizeVec(vec1);  // 20240110 Update
	NormalizeVec(vec2);

	dotResult = vec1 * vec2;  //点积

	if (abs(dotResult) < CountMinValue) {
		dotResult = 0;
	}
	if (dotResult >= 0) {
		return true;
	}
	else {
		return false;
	}
}
void removeLine(Line line, vector<Line>& lines) {
	int sizeVec;
	bool overLoop = true;

	sizeVec = lines.size();

	for (int i = 0; i < sizeVec; ++i) {
		if ((line.Pt1 == lines[i].Pt1 && line.Pt2 == lines[i].Pt2)
			|| (line.Pt1 == lines[i].Pt2 && line.Pt2 == lines[i].Pt1)
			) {
			lines.erase(lines.begin() + i); //
			//cout << "The Line :" << line << "removed!" << endl;
			overLoop = false;
			break;
		}
	}

	if (overLoop) {
		cout << "Error! The Line is not in the lines!" << endl;
	}
}
// 获取当前线段在线组中的位置
int getPosition(vector<Line>& lines, Line& line) {
	int index = -1;
	for (int i = 0; i < lines.size(); ++i) {    //定位推挤线在总体折线的位置
		if (line.Pt1 == lines[i].Pt1 && line.Pt2 == lines[i].Pt2) {
			index = i;
			line = lines[i];		//（补充）将line更新为lines中目标线段的索引
			break;
		}
	}
	return index;
}
void mergeLines(vector<Line>& lines) {
	//删除长度为0的线段，合并相邻且共线的线段
	for (int i = 0; i + 1 < lines.size(); ++i) {		//确保i+1不出界
		Point veci = lines.at(i).getVector();
		double Lengthi = lines.at(i).getLength();
		Point veci1 = lines.at(i + 1).getVector();
		double Lengthi1 = lines.at(i + 1).getLength();
		if (Lengthi < CountMinValue) {					//线段i长度为0，删除
			if (i == 0)
				lines.at(i + 1).Pt1 = lines.at(i).Pt1;
			else
				lines.at(i - 1).Pt2 = lines.at(i).Pt2;
			lines.erase(lines.begin() + i);
		}
		else if (Lengthi1 < CountMinValue) {			//线段i+1长度为0，删除，MapMinValue
			lines.at(i).Pt2 = lines.at(i + 1).Pt2;
			//维护走线，此处不需要维护lockLines
			lines.erase(lines.begin() + i + 1);
		}
		else if (abs(cross(veci / Lengthi, veci1 / Lengthi1)) < AngleMaxValue
			&& lines.at(i).width == lines.at(i + 1).width) {	//线段i与线段i+1长度均不为0，但共线，并且宽度相等，合并AngleMaxValue

			//维护走线
			lines.at(i).Pt2 = lines.at(i + 1).Pt2;
			lines.erase(lines.begin() + i + 1);
		}
	}
}



size_t v2Hash::PointHash::operator()(const Point& pnt) const 
{
	return std::hash<double>()(round(pnt.x * 1e6))^ std::hash<double>()(round(pnt.y * 1e6));
}

size_t v2Hash::LineHash::operator()(const Line& line) const 
{
	return PointHash()(line.Pt1) ^ PointHash()(line.Pt2);
}
